#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/opencv.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <iomanip>
#include <mutex>

class SensorMonitorNode
{
private:
    ros::NodeHandle nh_;
    
    // 订阅者
    ros::Subscriber cmd_vel_sub_;
    ros::Subscriber odom_sub_;
    ros::Subscriber rgb_sub_;
    ros::Subscriber depth_sub_;
    ros::Subscriber lidar_sub_;
    
    // TF监听器
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    
    // 数据存储
    geometry_msgs::Twist current_velocity_;
    nav_msgs::Odometry current_odom_;
    cv::Mat rgb_image_;
    cv::Mat depth_image_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr lidar_cloud_;
    
    // 数据状态
    bool has_velocity_data_;
    bool has_odom_data_;
    bool has_rgb_data_;
    bool has_depth_data_;
    bool has_lidar_data_;
    
    // 互斥锁
    std::mutex data_mutex_;

public:
    SensorMonitorNode() : tf_listener_(tf_buffer_), 
                         lidar_cloud_(new pcl::PointCloud<pcl::PointXYZ>),
                         has_velocity_data_(false), has_odom_data_(false),
                         has_rgb_data_(false), has_depth_data_(false), has_lidar_data_(false)
    {
        // 订阅话题
        cmd_vel_sub_ = nh_.subscribe("/cmd_vel", 10, &SensorMonitorNode::cmdVelCallback, this);
        odom_sub_ = nh_.subscribe("/odom", 10, &SensorMonitorNode::odomCallback, this);
        rgb_sub_ = nh_.subscribe("/camera/color/image_raw", 1, &SensorMonitorNode::rgbCallback, this);
        depth_sub_ = nh_.subscribe("/camera/depth/image_rect_raw", 1, &SensorMonitorNode::depthCallback, this);
        lidar_sub_ = nh_.subscribe("/rslidar_points", 1, &SensorMonitorNode::lidarCallback, this);
        
        ROS_INFO("松灵小车传感器监控节点已启动");
        ROS_INFO("正在等待传感器数据...");
    }

    void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        current_velocity_ = *msg;
        has_velocity_data_ = true;
    }

    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        current_odom_ = *msg;
        has_odom_data_ = true;
    }

    void rgbCallback(const sensor_msgs::Image::ConstPtr& msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        try
        {
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
            rgb_image_ = cv_ptr->image;
            has_rgb_data_ = true;
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("RGB图像转换错误: %s", e.what());
        }
    }

    void depthCallback(const sensor_msgs::Image::ConstPtr& msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        try
        {
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1);
            depth_image_ = cv_ptr->image;
            has_depth_data_ = true;
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("深度图像转换错误: %s", e.what());
        }
    }

    void lidarCallback(const sensor_msgs::PointCloud2::ConstPtr& msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        pcl::fromROSMsg(*msg, *lidar_cloud_);
        has_lidar_data_ = true;
    }

    void displayStatus()
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        
        std::cout << "\033[2J\033[1;1H"; // 清屏
        std::cout << "==================== 松灵小车传感器监控系统 ====================" << std::endl;
        std::cout << "时间: " << ros::Time::now() << std::endl;
        std::cout << "------------------------------------------------------------" << std::endl;
        
        // 显示传感器状态
        std::cout << "传感器状态:" << std::endl;
        std::cout << "  速度命令: " << (has_velocity_data_ ? "✓ 正常" : "✗ 无数据") << std::endl;
        std::cout << "  里程计: " << (has_odom_data_ ? "✓ 正常" : "✗ 无数据") << std::endl;
        std::cout << "  RGB相机: " << (has_rgb_data_ ? "✓ 正常" : "✗ 无数据") << std::endl;
        std::cout << "  深度相机: " << (has_depth_data_ ? "✓ 正常" : "✗ 无数据") << std::endl;
        std::cout << "  激光雷达: " << (has_lidar_data_ ? "✓ 正常" : "✗ 无数据") << std::endl;
        
        std::cout << "------------------------------------------------------------" << std::endl;
        
        // 显示速度信息
        if (has_velocity_data_)
        {
            std::cout << "当前速度命令:" << std::endl;
            std::cout << "  线速度: " << std::fixed << std::setprecision(3) 
                     << current_velocity_.linear.x << " m/s" << std::endl;
            std::cout << "  角速度: " << std::fixed << std::setprecision(3) 
                     << current_velocity_.angular.z << " rad/s" << std::endl;
        }
        
        // 显示里程计信息
        if (has_odom_data_)
        {
            std::cout << "里程计数据:" << std::endl;
            std::cout << "  位置: (" << std::fixed << std::setprecision(3)
                     << current_odom_.pose.pose.position.x << ", "
                     << current_odom_.pose.pose.position.y << ", "
                     << current_odom_.pose.pose.position.z << ") m" << std::endl;
            std::cout << "  线速度: " << std::fixed << std::setprecision(3)
                     << current_odom_.twist.twist.linear.x << " m/s" << std::endl;
            std::cout << "  角速度: " << std::fixed << std::setprecision(3)
                     << current_odom_.twist.twist.angular.z << " rad/s" << std::endl;
        }
        
        // 显示图像信息
        if (has_rgb_data_)
        {
            std::cout << "RGB图像: " << rgb_image_.cols << "x" << rgb_image_.rows << " 像素" << std::endl;
        }
        
        if (has_depth_data_)
        {
            std::cout << "深度图像: " << depth_image_.cols << "x" << depth_image_.rows << " 像素" << std::endl;
        }
        
        // 显示点云信息
        if (has_lidar_data_)
        {
            std::cout << "激光雷达点云: " << lidar_cloud_->points.size() << " 个点" << std::endl;
        }
        
        std::cout << "------------------------------------------------------------" << std::endl;
        std::cout << "按 Ctrl+C 退出程序" << std::endl;
        std::cout << "============================================================" << std::endl;
    }

    void run()
    {
        ros::Rate rate(5); // 5Hz
        
        while (ros::ok())
        {
            displayStatus();
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sensor_monitor_node");
    
    // 设置控制台编码为UTF-8以正确显示中文
    setlocale(LC_ALL, "zh_CN.UTF-8");
    
    SensorMonitorNode node;
    node.run();
    
    return 0;
}
